#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <unordered_map>
#include <unordered_set>
#include <algorithm>
#include <functional>
#include <memory>
#include <stdexcept>
#include <chrono>

// 2D 点结构
struct Point {
    int x, y;
    
    Point(int x = 0, int y = 0) : x(x), y(y) {}
    
    bool operator==(const Point& other) const {
        return x == other.x && y == other.y;
    }
    
    bool operator!=(const Point& other) const {
        return !(*this == other);
    }
    
    Point operator+(const Point& other) const {
        return {x + other.x, y + other.y};
    }
};

// 哈希函数特化
namespace std {
    template<> 
    struct hash<Point> {
        size_t operator()(const Point& p) const {
            return hash<int>()(p.x) ^ (hash<int>()(p.y) << 1);
        }
    };
}

// 节点结构
struct Node {
    Point pos;
    double g_cost;  // 从起点到当前节点的实际代价
    double h_cost;  // 到目标点的启发式代价
    double f_cost;  // 总代价 (g + h)
    std::shared_ptr<Node> parent;
    
    Node(Point p, double g, double h, std::shared_ptr<Node> parent = nullptr)
        : pos(p), g_cost(g), h_cost(h), f_cost(g + h), parent(parent) {}
    
    // 优先队列比较函数
    struct Compare {
        bool operator()(const std::shared_ptr<Node>& a, const std::shared_ptr<Node>& b) const {
            return a->f_cost > b->f_cost; // 小顶堆
        }
    };
};

// 地图类
class GridMap {
public:
    GridMap(int width, int height) 
        : width_(width), height_(height), grid_(height, std::vector<bool>(width, false)) {}
    
    // 设置障碍物
    void setObstacle(int x, int y, bool is_obstacle = true) {
        if (x >= 0 && x < width_ && y >= 0 && y < height_) {
            grid_[y][x] = is_obstacle;
        }
    }
    
    // 检查是否为障碍物
    bool isObstacle(const Point& p) const {
        if (p.x < 0 || p.x >= width_ || p.y < 0 || p.y >= height_) {
            return true; // 边界外视为障碍物
        }
        return grid_[p.y][p.x];
    }
    
    int width() const { return width_; }
    int height() const { return height_; }

private:
    int width_, height_;
    std::vector<std::vector<bool>> grid_; // true 表示障碍物
};

// 启发式函数类型
enum class HeuristicType {
    MANHATTAN,
    EUCLIDEAN,
    DIAGONAL,
    CHEBYSHEV
};

// A* 路径规划器
class AStarPlanner {
public:
    AStarPlanner(std::shared_ptr<GridMap> map, HeuristicType h_type = HeuristicType::EUCLIDEAN)
        : map_(map), heuristic_type_(h_type) {}
    
    // 主规划函数
    std::vector<Point> findPath(const Point& start, const Point& goal) {
        auto start_time = std::chrono::high_resolution_clock::now();
        
        // 验证输入
        if (map_->isObstacle(start)) {
            throw std::invalid_argument("Start point is in obstacle or out of bounds");
        }
        
        if (map_->isObstacle(goal)) {
            throw std::invalid_argument("Goal point is in obstacle or out of bounds");
        }
        
        // 初始化
        open_set_.clear();
        closed_set_.clear();
        node_cache_.clear();
        
        // 创建起始节点
        auto start_node = std::make_shared<Node>(start, 0.0, heuristic(start, goal));
        open_set_.push(start_node);
        node_cache_[start] = start_node;
        
        // 允许的移动方向 (8方向)
        const std::vector<Point> directions = {
            {0, 1}, {1, 0}, {0, -1}, {-1, 0},  // 上下左右
            {1, 1}, {1, -1}, {-1, -1}, {-1, 1} // 对角线
        };
        
        // A* 主循环
        while (!open_set_.empty()) {
            auto current = open_set_.top();
            open_set_.pop();
            
            // 找到目标
            if (current->pos == goal) {
                auto end_time = std::chrono::high_resolution_clock::now();
                auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
                std::cout << "Path found in " << duration.count() << " ms\n";
                return reconstructPath(current);
            }
            
            // 添加到关闭列表
            closed_set_.insert(current->pos);
            
            // 探索邻居
            for (const auto& dir : directions) {
                Point neighbor_pos = current->pos + dir;
                
                // 跳过障碍物和已访问节点
                if (map_->isObstacle(neighbor_pos) || 
                    closed_set_.find(neighbor_pos) != closed_set_.end()) {
                    continue;
                }
                
                // 计算移动成本 (直行1.0, 对角线√2)
                double move_cost = (abs(dir.x) + abs(dir.y) == 2) ? M_SQRT2 : 1.0;
                double new_g_cost = current->g_cost + move_cost;
                
                // 检查节点是否已存在
                auto it = node_cache_.find(neighbor_pos);
                if (it == node_cache_.end()) {
                    // 新节点
                    double h_cost = heuristic(neighbor_pos, goal);
                    auto neighbor_node = std::make_shared<Node>(
                        neighbor_pos, new_g_cost, h_cost, current);
                    open_set_.push(neighbor_node);
                    node_cache_[neighbor_pos] = neighbor_node;
                } else {
                    // 已有节点，检查是否找到更好路径
                    auto neighbor_node = it->second;
                    if (new_g_cost < neighbor_node->g_cost) {
                        neighbor_node->g_cost = new_g_cost;
                        neighbor_node->f_cost = new_g_cost + neighbor_node->h_cost;
                        neighbor_node->parent = current;
                        
                        // 更新优先队列：简单实现 (实际应用中应使用可减少键操作的堆)
                        // 对于小规模地图足够高效
                        std::priority_queue<std::shared_ptr<Node>, 
                                           std::vector<std::shared_ptr<Node>>, 
                                           Node::Compare> new_queue;
                        while (!open_set_.empty()) {
                            new_queue.push(open_set_.top());
                            open_set_.pop();
                        }
                        open_set_ = std::move(new_queue);
                    }
                }
            }
        }
        
        throw std::runtime_error("No path found");
    }

private:
    // 重构路径
    std::vector<Point> reconstructPath(std::shared_ptr<Node> end_node) {
        std::vector<Point> path;
        auto current = end_node;
        
        while (current != nullptr) {
            path.push_back(current->pos);
            current = current->parent;
        }
        
        std::reverse(path.begin(), path.end());
        return path;
    }
    
    // 启发式函数
    double heuristic(const Point& a, const Point& b) const {
        int dx = abs(a.x - b.x);
        int dy = abs(a.y - b.y);
        
        switch (heuristic_type_) {
            case HeuristicType::MANHATTAN:
                return dx + dy;
                
            case HeuristicType::EUCLIDEAN:
                return std::sqrt(dx*dx + dy*dy);
                
            case HeuristicType::DIAGONAL:
                return (dx + dy) + (M_SQRT2 - 2) * std::min(dx, dy);
                
            case HeuristicType::CHEBYSHEV:
                return std::max(dx, dy);
                
            default:
                return dx + dy;
        }
    }

    // 地图引用
    std::shared_ptr<GridMap> map_;
    
    // 启发式类型
    HeuristicType heuristic_type_;
    
    // 开放列表 (优先队列)
    std::priority_queue<std::shared_ptr<Node>, 
                        std::vector<std::shared_ptr<Node>>, 
                        Node::Compare> open_set_;
    
    // 关闭列表 (哈希集合)
    std::unordered_set<Point> closed_set_;
    
    // 节点缓存 (快速查找)
    std::unordered_map<Point, std::shared_ptr<Node>> node_cache_;
};

// 打印地图和路径
void visualizePath(const GridMap& map, const std::vector<Point>& path, 
                   const Point& start, const Point& goal) {
    std::vector<std::vector<char>> display(
        map.height(), std::vector<char>(map.width(), '.'));
    
    // 标记障碍物
    for (int y = 0; y < map.height(); ++y) {
        for (int x = 0; x < map.width(); ++x) {
            if (map.isObstacle({x, y})) {
                display[y][x] = '#';
            }
        }
    }
    
    // 标记路径 (起点和终点除外)
    for (size_t i = 1; i < path.size() - 1; ++i) {
        display[path[i].y][path[i].x] = '*';
    }
    
    // 标记起点和终点
    display[start.y][start.x] = 'S';
    display[goal.y][goal.x] = 'G';
    
    // 打印地图
    for (int y = 0; y < map.height(); ++y) {
        for (int x = 0; x < map.width(); ++x) {
            std::cout << display[y][x] << ' ';
        }
        std::cout << '\n';
    }
}

int main() {
    try {
        // 创建 10x10 地图
        auto map = std::make_shared<GridMap>(10, 10);
        
        // 设置障碍物
        for (int i = 2; i < 8; ++i) map->setObstacle(i, 5);
        map->setObstacle(5, 2);
        map->setObstacle(5, 3);
        map->setObstacle(5, 4);
        map->setObstacle(5, 6);
        map->setObstacle(5, 7);
        
        // 创建规划器 (使用对角线启发式)
        AStarPlanner planner(map, HeuristicType::DIAGONAL);
        
        // 设置起点和终点
        Point start(1, 1);
        Point goal(8, 8);
        
        // 查找路径
        auto path = planner.findPath(start, goal);
        
        // 打印路径
        std::cout << "\nPath (" << path.size() << " points):\n";
        for (const auto& p : path) {
            std::cout << "(" << p.x << ", " << p.y << ") ";
        }
        std::cout << "\n\n";
        
        // 可视化路径
        visualizePath(*map, path, start, goal);
        
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }
    
    return 0;
}